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ABSTRACT 

A  discrete  state  space  model  is  developed  which  describes  an  autonomous  under¬ 
water  vehicle  and  incorporates  the  effects  of  currents,  sea  state,  and  wind  as  it  travels 
through  the  sea.  Heading  commands  are  calculated  to  overcome  these  effects  by  the 
design  of  an  Extended  Kalman  Filter  which  estimates  their  combined  velocity  compo¬ 
nents.  Simulations  are  done  which  test  the  filter's  effectiveness  in  a  range  of  different 
environments.  Some  potential  uses  for  this  system  are  discussed  at  the  end. 
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I.  INTRODUCTION 


A.  BACKGROUND 

With  the  increased  costs  for  Manned  Naval  Suface  and  Subsurface  assets  and  the 
greatly  increased  capability  of  small  computing  devices,  using  small  autonomous  under¬ 
water  vehicles  (ALVs)  to  carryout  certain  missions  has  become  increasingly  desirable. 
A  vehicle's  capability  to  navigate  to  a  precise  location  independently  and  in  many  cases 
covertly  is  essential. 

Current  research  into  potential  navigation  systems  that  measure  a  vehicle  s  position 
and  unknown  forces  acting  on  it  include  sonar  doppler,  stapdown  Inertial  Measuring 
Units  (IMUs),  and  sonar  triangularization.  The  two  primary  IMUs  underdevelopment 
for  AL’Vs  are  the  Laser  Ring  Gyro  and  the  Fiber  Optic  Gyro  (FOG).  The  systems 
mentioned  above  each  have  their  own  independent  drift  characteristics  which  have  to 
be  filtered  out  and  their  actual  positions  updated  from  time  to  time  using  an  outside 
source  such  as  the  Navstar  Global  Positioning  System  (GPS)  or  Omega.  In  addition, 
they  have  large  power  requirements,  are  bulky  (with  the  possible  exception  of  the  FOG), 
and  require  compex  control  circuitry.  If  a  microprocessor  could  be  programmed  to  es¬ 
timate  the  underwater  forces  acting  on  the  vehicle  enroute  to  the  destination,  based  on 
updates  by  GPS,  and  then  to  apply  a  heading  command  to  compensate  for  any  devi¬ 
ations  in  its  course,  then  the  power  requirements,  complexity,  and  cost  could  be 
signifcantly  reduced.  Figure  1  on  page  2  illustrates  a  block  diagram  of  a  possible  sys¬ 
tem. 

In  this  Thesis,  a  state  space  model  will  be  developed  based  on  a  generic  vehicle's 
dynamics.  The  forces  acting  on  the  vehicle  will  be  modeled  as  a  velocity  vector  with  an 
angle  (Drift)  and  a  magnitude  (Set)  with  the  following  assumptions: 

•  the  angle  and  magnitude  of  this  vector  are  white  random  Gaussian  variables  with 
zero  mean  about  predicted  values,  and 

•  the  overall  Set  and  Drift  between  sample  intervals  remains  a  constant. 

In  order  to  estimate  this  set  and  drift,  an  extended  Kalman  Filter  is  developed. 
Various  simulations  are  performed  which  explore  the  behavior  of  the  filter  and  its  ability 
to  estimate  the  velocity  components.  The  simulation  was  done  using  DSL  (Dynamic 
Simulation  Language)  on  the  Naval  Postgraduate  School  s  IBM  3033  Mainframe  Com- 
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Figure  1.  System  Block  Diagram 
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B.  GLOBAL  POSITIONING  SYSTEM 


The  Xavstar  Global  Positioning  System  is  a  space-based  radio  navigation  system 
which  operates  passively.  GPS  is  a  tri-service  multiagency  program  managed  by  the  Air 
Force's  GPS  Joint  Program  Office.  When  fully  operational  in  the  early  1990's,  the  sys¬ 
tem  will  consist  of  18  satellites  with  three  orbiting  spares,  in  6  orbital  planes.  Each  sat¬ 
ellite  transmits  two  unique  codes,  the  clear,  acquision  (C/A)  code  and  the  Precise  (P) 
code.  These  codes,  which  are  unique  to  each  satellite,  are  modulated  and  two  L  Band 
frequencies,  LI  and  L2.  as  pseudo  random  noise  sources  along  with  the  Navigation 
Message.  LI  contains  both  the  C.  A  and  P  codes  while  L2  has  just  the  P  code. 

The  user's  receiver  generates  the  identical  codes  and  correlates  its  code  with  the 
satellite's.  The  amount  of  slewing  theat  the  receiver  must  do  in  order  to  obtain  a  match 
is  the  time  that  the  two  signals  are  off.  Dividing  by  the  speed  of  light  and  correcting  for 
atmospheric  delays  and  any  clock  differences  produces  the  range  to  the  satellite.  Ob¬ 
taining  three  ranges  from  three  different  satellites  is  required  to  obtain  a  3  dimensional 
coordinate  position  fix.  However,  in  order  to  accomplish  this,  the  receiver's  clock  would 
have  to  be  identical  in  time  to  that  of  the  GPS's  Master  Clock  which  would  defeat  the 
purpose  of  the  system.  To  provide  this  time  differential,  the  receiver  must  lock  onto  4 
satellites  and  solve  four  simultaneous  equations  for  the  three  coordinates  and  the  time 
differential  in  order  to  get  the  fix. 

The  GPS  Navigation  Message  supplies  the  user  with  the  satellite's  clock  offset,  at¬ 
mospheric  delays,  and  satellite  ephtmeris  in  order  to  accurately  solve  the  four  equations. 
It  is  modulated  on  both  LI  and  L2  at  a  50  bit  per  second  rate  and  is  contained  in  a  1500 
bit  frame.  This  frame  is  divided  into  5  subframes  which  contain  specific  blocks  of  data. 
This  format  is  shown  in  Figure  2  on  page  4. 

The  first  subframe  contains  the  dock  correction  parameters  and  ionospheric  propa¬ 
gation  delay  model  parameters.  The  second  and  third  subframes  contain  the  satellite's 
ephemeris  which  is  updated  every  1-5  hours  by  the  ground  control  segment  of  GPS. 
Subframe  four  is  for  any  special  messages  which  must  be  passed  onto  the  users. 

The  final  subframe  contains  the  almanac  data.  The  almanac  data  is  an  abbreviated 
form  of  the  first  three  subframes  for  each  of  the  satellites.  This  data  is  transmitted  on 
a  rotating  basis  and  requires  19  subframes  to  transmit  it  all  and  is  used  only  to  help  lo¬ 
cate  the  four  satellites  which  are  overhead.  The  almanac  must  be  loaded  into  the  re¬ 
ceiver  either  manually  or  by  the  receiver  itself  by  locking  onto  a  satellite  and  obtaining 
the  block  five  message  through  an  entire  cycle. 
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C.  GPS  RECEIVER 

In  order  to  perform  the  tasks  that  will  be  referred  to  later  in  this  thesis,  the  GPS 
receiver  must  be  capable  of  accomplishing  the  following  tasks: 

•  Obtain  current  position 

•  Store  the  necessary  waypoints,  including  the  start  and  end  points. 

•  Provide  course  information  to  next  waypoint. 

•  Have  the  ability  to  survive  temperature,  pressure,  and  other  conditions  associated 
with  the  marine  environment. 

Currently  under  development  for  the  Marine  Corps  by  the  Naval  Ocean  Systems 
Command  (NOSC)  San  Diego  is  a  receiver  called  the  Small  Unit  Navigation  System 
(SUNS).  [Ref.l)  This  receiver  is  designed  to  be  handheld,  rugged,  lightweight,  and  bat¬ 
ten.-  operated  for  the  Marine  Corp's  Reconnaissance  teams  during  long  range  operations 
on  both  land  and  underwater.  SUNS  will  be  able  to  store  up  to  waypoints,  including 
the  destination,  and  will  provide  the  user  with  current  position,  velocity,  cross  track  er¬ 
ror.  distance  and  bearing  to  next  waypoint,  and  time  with  a  positional  accuracy  of  50  ft. 

One  additional  requirement  which  should  be  considered  is  equipping  the  receiver 
with  a  multi-channel  capability  to  lock  on  all  four  satellites  at  the  same  time.  This  would 
allow  the  vehicle  to  obtain  a  position  fix  quicker  and  thus  spend  less  time  on  or  near  the 
surface.  The  SUNS  receiver  is  being  designed  as  a  single  channel  unit  and  will  have  to 
sequentially  correlate  each  of  the  signals  from  the  four  satellites. 


II.  SYSTEM  MODEL 


A.  DYNAMIC  MODEL 

An  underwater  vehicle  will  travel  through  the  water  with  a  desired  heading  and  at 
an  ordered  speed.  The  combined  external  forces  acting  on  the  vehicle  will  cause  it  to 
deviate  from  the  intended  track  at  some  angle  (Drift)  and  velocity  (Set).  This  deviation 
can  be  modeled  as  a  single  velocity  vector  when  added  to  the  vehicle's  own  velocity 
vector  results  in  the  True  Velocity  vector  along  which  the  vehicle  will  travel.  This  re¬ 
lationship  is  shown  in  Figure  3  on  page  7,  where  a  is  the  vehicle's  commanded  heading. 
y  is  the  angle  for  the  True  Velocity,  and  1 1/  is  the  angle  of  the  drift's  velocity.  The  simple 
addition  of  the  indvidual  components  produce  Equations  ‘  .  and  2.2  for  the  \  and  y 
components  of  Vt,  .vr  and  yr. 

xT  =  Vel cos(a)  +  Set  cos(> l/)  (2.1) 

yT—  Vel  sin(a)  +  Set  sin(i^)  (2.2) 

Developed  in  Reference  2,  the  vehicle's  dynamic  model,  neglecting  any  sway  dy¬ 
namics.  is  illustrated  in  Figure  4  on  page  8.  In  this  model,  a  heading  input  (Hdg) 
produces  a  heading  direction  (  a  ),  whose  sine  and  cosine  are  multiplied  by  the  vehicle's 
ordered  speed  (Vel)  to  produce  the  ordered  velocity  components  (.v  and  y).  If  these 
components  are  summed  with  their  respective  drift  components  ( ux  and  ny),  then  the 
components  of  the  true  velocity  (x,  and  y,)  are  obtained. 

G(s)  is  the  transfer  function  describing  the  vehicle's  true  dynamics,  which  for  the 
generic  vehicle  used  in  this  thesis  will  be  a  simplified  first  order  system.  With  k  =  4.4  and 
O(s)  =  —  ,  a  transient  response  as  shown  in  Figure  5  on  page  9  is  realized 

IIDGCOM  is  the  applied  heading  command  and  ALPHA  is  the  resultant  vehicle  head¬ 
ing  angle  (  a  ).  This  G(s)  was  chosen  as  a  reduced  order  model  based  on  various  vehicles 
which  are  now  in  operation.  K  =  4.4  produces  a  2  sec.  settling  time  with  little  overshoot. 
This  type  of  response  will  minimize  the  effect  of  the  vehicle's  response  to  heading  com¬ 
mands  on  the  Kalman  Estimator  updating  during  the  simulations  in  Chapter  4  and  is 
close  to  what  a  real  vehicle  would  produce. 

As  can  be  seen  from  the  model,  this  is  an  open  loop  system  where  the  heading  angle 
is  put  in  from  an  outside  source  and  maintained  by  the  position  feedback  of  a  .  This 
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Figure  3.  Velocity  Components  Encountered  by  the  Vehicle 
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heading  command  will  be  provided  by  the  processor  after  calculating  ux  and  ay  and  de¬ 
termining  an  a  which  will  negate  these  inputs. 

B.  STATE  SPACE  REPRESENTATION 

In  this  section  a  discrete  state  space  model  is  developed  based  on  the  dynamic  model 
above  along  with  the  identification  of  the  individual  states. 

The  linear  discrete  state  space  model  is  described  by  Equations  2.3  and  2.4  [Ref.3: 
pp.  66-67]  where  &  is  the  current  value  of  the  states  at  time  =  t  and  contains  the 
predicted  values  of  the  states  at  time  =  t  4-  T.  T  is  defined  as  the  sampling  period.  The 
definitions  for  the  variables  in  the  equations  below  are  summarized  in  Figure  6. 

=£/,•+,  =  +  r ,  uk  +  r  2wk  (2.3) 

*  =  Hxk  +  Vk  ( 2 .H) 


Figure  6.  Symbol  Identification 


The  following  states  were  defined  and  placed  into  & 
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,v(5)  =  u.x 


.y(6)  =  uy 


The  O  Matrix  becomes 


(2.0c) 


1  T  0  0  0  0 
0  1  0  0  0  0 
0  0  1  T  0  0 
0  0  0  1  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 


The  command  vector,  uk,  will  consist  of  the  velocity  components,  u: xk  and  uyk,  of  any 
new  heading  given  to  the  vehicle  which  are  needed  to  compensate  for  the  estimated  set 
and  drift.  They  are  given  in  the  equations  below  with  Hdgk  and  Hdgk. {  as  the  heading 
commands  at  time=  kT  and  (k-l)T  respectively. 

u.xk  =  Vel  x  (  cos {Hdgk)  -  cos {Hdgk_x))  (2.7) 

uyk  =  Vel  x  ( sin (IIdgk)  -  sin(//<^ft_,))  (2.8) 

T,  now  becomes: 


0  T 


Tlie  x  and  y  components  of  the  Set  and  Drift  are  contained  in  v&  as  vv.r  and  wy. 
Since  the  Set's  magnitude  and  the  Drift's  direction  are  assumed  to  be  white  Gaussian 
random  variables  with  zero  mean  about  predicted  values,  iv.v  and  wy  will  also  be  white 
Gaussian  random  variables  with  zero  mean  about  their  predicted  values.  This  leads  to 
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(2. 10) 


At  this  point,  the  observation  vector,  z,  consists  only  of  the  position  coordinates 
{x  and  y)  which  come  from  GPS.  The  velocity  components  received  from  GPS  are  those 
components  that  the  vehicle  is  experiencing  at  that  instant  in  time  and  may  or  may  not 
be  the  time  averaged  velocities  which  are  defined  in  the  model.  If  only  the  position  co¬ 
ordinates  are  used  then  the  system  is  not  observable  [Ref.  3:  pp  67-70|.  As  a  result  an¬ 
other  variable  must  be  measured. 

In  the  system  model  (  Figure  4  on  page  8  ),  recall  that  a  is  fedback  to  the  input  in 
order  to  maintain  the  applied  heading  command.  Since  this  value  must  be  measured,  it 
can  serve  as  the  other  variable  that  will  link  the  states.  Thus  the  observation  vector 
becomes: 


(HI) 


2=  J,  +i’ 


It  can  be  observed  that  z  is  now  nonlinear  because  a  is  not  a  linear  function  of  the 
states  .r(2),  .t(4),  .v(5).  and  ,v(6).  [Eq.  2. 1 2 1  As  a  result,  it  cannot  be  treated  as  the  linear 
function  as  described  in  Equation  2.4. 


a  =  arcian 


~  ,v(4)  —  ,t(6)  - 
-v(2)  -  -v(5) 

_ 


(2-12) 


The  next  step  is  to  create  an  estimator  that  will  measure  ux  and  uy  in  the  presence 
of  noise  (uncertainty  )  in  the  measurements  and  overcome  nonlinear  problem  of  the  ob¬ 
servation  vector.  In  Chapter  3,  this  is  accomplished  by  the  design  of  an  extended 
Kalman  Filter. 
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III.  STATE  ESTIMATION 


A.  INTRODUCTION 


In  this  chapter,  the  theory  of  statistical  parameter  estimation  is  applied  to  deter¬ 
mining  the  values  of  the  Set  and  Drift  components,  ilx  and  uy,  which  were  discussed  in 
Chapter  2.  Because  of  the  nonlinear  measurement  vector  and  the  uncertainty  generated 
in  the  measurement  noise,  a  simple  observer,  i.e.,  the  Luenberger  Observer,  is  not  feasi¬ 
ble.  Due  to  the  successful  applications  of  the  Extended  Kalman  Filter  [Ref.  4:  p.  23]  in 
solving  nonlinear  estimation  problems  and  the  advancements  in  microprocessor  design 
in  obtaining  real  time  solutions,  the  Extended  Kalman  Filter  was  chosen  as  the  estimator 
for  this  problem. 

B.  KALMAN  FILTER 

1.  The  Linear  Kalman  Filter 

The  Kalman  Filter  is  an  algorithm  for  extracting  or  estimating  information  from 
noisy  data.  More  specifically,  it  is  a  parametric  estimation  process  based  on  an 
autoregresive  (AR)  model  of  the  plant.  The  filter  uses  samples  or  measurements  of  the 
plant  as  observables  to  recursively  update  estimates  of  the  plant's  state. 

When  both  the  system  and  measurement  models  are  linear  functions  of  the 
states.  The  state  update  equation  is  shown  below  in  Equations  3.1  and  3.2.  In  these 
equations  G  is  the  vector  containing  the  optimum  estimation  gains  (Kalman  gains). 

is  the  predicted  value  of  the  states  at  time  =  k  T  based  on  the  measurements  ob¬ 
tained  and  any  known  forcing  functions  at  time  =  (k  —  1)T,  and  i*  is  the  updated  state 
estimation  vector  based  on  the  new  measurement  vector  (r*). 

£k  k  ~  k—  I  ^(“/<  —  U-^-k  k—  l)  (JT) 

The  following  assumptions  are  made  in  using  the  Kalman  equations: 

•  The  random  forcing  function  (u’t)  is  zero  mean  and  uncorrelated  with  covariance 

Q\. 

ZT[w;]  =0,  for  all  k  >  0  ,  ^ 

rrw  ...  1  -  f  Q'k.k^kk  lor  all  k  =/] 

fQr  aU  k^j  J 

•  The  measurement  noise  (rj  is  zero  mean  and  uncorrelated  with  covariance  Rt. 


SKEW 
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£[vv]  =  0.  for  all  k>0 

£f  .  ..  I  _  f"  Rkk^kk  {'or  a11  k  =j  1 
— k~j-^  [_0  lor  all  k=£j  J 

•  The  random  forcing  function  and  measurement  noise  are  uncorrellated. 

=  £Q>Hyr3  =  0  for  all  kj  =£  0 

•  The  random  forcing  function  and  initial  states  are  uncorrellated. 

£f  u-7cr0r~l  =  ELxqvvJ]  =  0  for  all  k  =£  0 

•  The  measurement  noise  and  initial  states  are  uncorrellated. 

£[i'/,~.I0r ]  =  =  0  for  all  k  ¥=  0 


(3.3) 

(3.4) 

(3.5) 

(3.6) 


The  optimal  estimation  gains  are  those  which  satisfy  the  following  relationships: 


kk  =  Pkk-\HKHkPkk-\HI  +  ^ 


-l 


(3.71 


^kk  ~  f-f  GkHk~\Pkk_  1 


PM-i-<i>PiiPT+r2<?rI 


(3.8) 

(3.9) 


where  Pk  *  is  the  error  covariance  matrix  update  at  time  =  k  T  and  ,  is  the  predicted 
error  covariance  at  time  =  (k  +  1)7  based  on  the  data  at  time  =  k  T. 

2.  Extended  Kalman  Filter 

As  previously  mentioned,  the  measurement  vector  as  described  in  Equations 
2.11  and  2.12  is  a  nonlinear  functon  of  the  states  and  as  a  result  will  be  modeled  as 
shown  below, 


zk  =  h(xk,k)  +  vk  (3.10) 

In  this  model,  the  measurement  vector,  zk,  is  a  function  of  the  state  variables 
plus  the  measurement  noise  vector,  Vk  The  adaptation  of  the  Kalman  Filter  involves  the 
linearizing  of  h(£k,k)  by  taking  the  Taylor  Series  expansion  about  the  predicted  value  of 
the  states,  jc*  at  time  =  k  T  and  keeping  only  the  first  order  terms.  This  is  the  Ex¬ 
tended  Kalman  Filter.  The  reader  is  referred  to  Geib  [Ref.  3:  pp  ISO-225)  for  a  detailed 
explanation  of  the  process.  In  summary,  a  more  precise  approximation  can  be  achieved 
in  the  optimal  estimator  by  including  higher  terms  of  the  series  expansion  but  at  the 
expense  of  simplicity  and  increasing  the  possibility  of  the  recursion  diverging. 

The  linearized  form  of  Equation  3.10  yields 
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•■k  Hk  -k 


(3.11) 


Hk- 


ch(xk,k) 


(3.12) 


A  summarv  of  the  definitions  for  the  Extended  Kalman  Filter  estimator  for  the 


linear  state  model  (Eq.  2.3]  and  the  linearized  measurement  model  [Eq.  3.1 1]  is  displayed 
in  Figure  7  on  page  16.  A  block  diagram  that  illustrates  the  algorithm  for  recursively 
computing  the  filter  is  shown  in  Figure  8  on  page  17. 


C.  MATRIX  CALCULATIONS 


1.  Determination  of  O' 


O'  as  defined  in  Equation  3.2  is  the  covariance  matrix  for  the  random  forcing 
function  of  the  set  and  drift  velocity  components.  It  is  a  measure  of  the  uncertainty 
contained  in  the  predictions.  As  stated  previously,  the  set  and  drift  experienced  by  the 
vehicle  is  assumed  to  be  Gaussian  with  0  mean  about  the  predicted  values  and  as  a  result 
their  x.y  components  are  independent  and  will  be  approximately  Gaussian  with  0  mean 
with  the  standard  deviations  equal  to  the  maximum  expected  deviation  in  the  x  and  y 
directions.  Thus  O’ 


(3.16) 


The  method  used  as  an  approximation  to  calculate  a2  and  a2  ,  is  the  following: 


•  Determine  the  maximum  amount  of  variation  which  can  be  expected  in  the  set  and 
drift. 


•  Calculate  the  cartesian  components,  x  and  y,  from  the  values  determined  above. 
Set  these  values  equal  to  3(7,,, 


•  Divide  the  x  and  y  components  by  3,  subtract  them  from  their  respective  predicted 
components,  and  square  to  obtain  a2wt  and  o2n . 


As  an  example,  let  the  maximum  expected  deviation  for  the  drift  be  ±  20°  and 
the  maximum  expected  deviation  for  the  set  be  ±  .5  knots.  The  predicted  values  for  the 
Set  and  Drift  arc  45°  and  1.0  knots.  Using  the  above  procedure, 


cri,  =  0.43  and  a2wy  =  0.43. 


I 
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1 

SYSTEM  MODEL:  xk+]  =  Oxfe  +  r,ufe  +  r2wk 

where  wk  cc  A'[O,0'] 

(13) 

I 

1 

» 

MEASURE  MODEL:  :k  =  h(xkJc)  +  vk 

where  wk  oc  A'C0,(?'] 

(3.10) 

1, 

INITIAL  CONDITIONS:  ^  oc  *[*,£,] 

(3.13) 

; 

OTHER  ASSUMPTIONS:  £[wfci/]  =  0  for  all  kJ^O. 

(3.4) 

| 

GAIN  EQUATION: 

(3.7) 

1 

• 

5 

Gk  =  Pk  HkPk  k_J[[  +RT' 

r 

r 

i 

) 

1 

ERROR  COVARIANCE  UPDATE: 

Pk.k  =  C  /  —  GkHk]Pk  k_\ 

(3.S) 

s 

STATE  ESTIMATION  UPDATE  EQUATION 

(3.1) 

(. 

V 

s 

Z-k  k  ~  &k  k- 1  "**  ~  ll{iikk_\\\ 

1 

• 

1 

ERROR  COVARIANCE  PROPAGATION  EQUATION: 

PkMk  =  ®Pkk(t>T+Q 

(3.9) 

f 

I 

STATE  ESTIMATION  PROPAGATION: 

-N-+1  k  —  k  +  r  1  Uk 

(3.16) 

■ 

c 

1 

rh(.xk,k) 

DEFINITIONS:  JJk  =  — ; - - - - 

CXk  i  = 

(3.12) 

1 

Q  =  f2qtI 

(3.14) 

( 

i 

i 

i 

— lAitr  1 )  =  1  ;* 

(3.15) 

Figure  7.  Summary  of  Extended  Kalman  Equations 
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2.  Determination  of  R 


R  as  defined  in  Equation  3.3,  is  the  covariance  matrix  for  the  measurement 
noise  vector.  As  in  Q' .  R  is  the  uncertainty  contained  in  the  measuring  of  the  three  pa¬ 
rameters  x,  y.  and  a  in  the  presence  of  noise.  For  this  application,  the  noise  for  all  three 
parameters  is  assumed  to  be  uncorrellated  amongst  themselves  and  white  Gaussian  with 
0  mean.  R  becomes 


R  = 


o~x  0 


0 

0 


(3.17) 


The  variances.  <r,  and  ay,  are  the  standard  deviations  from  GPS  and  are  obtained 
by  using  the  stated  DOD  value  for  the  Circular  Error  of  Probability  (CEP)  of  10  meters 
or  whichever  CEP  figure  is  associated  with  the  GPS  Receiver  in  use  and  Equation  3.18 
[Ref.  5:  p.  21].  The  standard  deviation  for  the  specific  type  of  compass  used  is  repres¬ 
ented  by  <tu. 

CUP  =  0.59K  +  <rv)  (3.  IS) 


3.  Determination  of  wi+llA 

iii.i  as  defined  in  Equation  3.1  is  a  predictor  of  the  states  at  time  =  k  +  1  based 
on  the  current  estimation  of  the  states  at  time  =  k  As  a  result.  xk.Zk  has  to  take  into 
account  any  controls  which  are  applied  as  a  new  heading  command  to  overcome  the 
estimated  set  and  drift  as  well  as  the  known  plant  dynamics.  Thus  is.,  „  becomes 

h+\  k  =  (I>4-  +  F \Uk  (3.19) 


where  T,.  <f>,  and  uk  were  defined  in  Equation  2.3. 

4.  Determination  of  Hk 

II,  as  defined  in  Equation  3.12  is  the  linearized  observation  vector  and  is  ob¬ 
tained  by  taking  the  partial  derivative  of;  with  respect  to  £.  Recalling  Equation  2.1 1. 
the  first  two  values  of;,  x  and  y.  are  linear  functions  of  only  one  state  and  their  partial 
derivatives  are  easily  computed  in  Equations  3.20  and  3.21. 
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.vf4)  -  .v(6) 


x(2)  -  x(5)  J 


r4^-  =  ■—-  =  [100000] 

c'A  cx 


(3.20) 


cr(2)  cy 


CX  CJC 


=  [0  010  0  0] 


(3.21) 


To  obtain  the  partial  derivative  of  z(3),  the  following  relationships  are  used. 
[Ref.  6:  pp  32S-330] 


d  arctani  u) 


d  u 


d  x 


1  +  u1  dx 


(3.22) 


d  u 


-l 


—  1  dtl 


d  X 


d  dx 


(3.23) 
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Using  Equations  3.21  and  3.22  and  simplifvine,  the  folio  win  a  results  were 
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_ 43)  —  4  2) _ 

(42)  -  -v(5))'  +  (.v(4)  -  .vie))2 

_ .v(6)  -  ,v(4) _ 

(42)  -  .v(5))2  +  (.v(4)  -  x(6))2 


(3.28) 


(3-29) 


The  above  equations  are  combined  into  Equation  3.30.  which  is  the  matrix  ver¬ 
sion  for  LLk.  The  final  version,  it  should  be  noted,  contains  only  three  major  calculations: 
the  denominator  term  which  is  common,  the  ,v(2)  —  .v(5)  term,  and  the  .v(4)  —  x(5)  term. 
The  two  terms,  //,,  and  //3a  are  just  the  negatives  of  H32  and  Hu  respectively.  Thus  this 
linearization  is  not  as  ominous  as  one  would  expect  and  can  easily  be  implemented  on 
a  microprocessor. 


where 


10  0  0  0  0 
Z4=  0  0  1  0  0  0 
0  IIn  0  Hu-Hn-Ilu 


H  _ _ 42)  -  .v(5) _ 

32  (.v(2)  -  .v(5))2  4-  (44)  -  -v(5))2 
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IV.  SIMULATION  RESULTS 


A.  INTRODUCTION 

In  this  Chapter,  the  Extended  Kalman  Filter  calculated  in  Chapter  3  is  tested  by 
performing  various  simulations  using  the  models  developed  in  Chapter  2  and  determin¬ 
ing  if  the  filter  is  correctly  estimating  the  six  states.  This  testing  was  done  in  four  steps. 
In  the  first  step,  the  filter  and  program  were  checked  to  insure  that  the  filter  accurately 
estimated  the  states  and  that  the  program  interacted  with  the  filter  while  maintaining  the 
vehicle's  track  with  no  added  bias.  Next,  a  bias  was  added  to  the  predicted  values  of  Set 
and  Drift  and  a  heading  change  is  made  to  correct  for  the  bias  and  allow  the  vehicle  to 
continue  to  its  destination.  This  test  would  show  whether  the  filter  would  accept  the 
heading  change  and  maintain  its  lock.  Third,  the  filter's  ability  to  handle  multiple  Set 
and  Drift  changes  is  checked  by  randomly  changing  the  bias  every  other  sample.  Last, 
random  noise  is  added  to  the  measurements  being  fed  into  the  filter  and  the  errors 
measured. 

The  full  simulation  program  which  was  used  to  run  the  last  simulation  is  shown  in 
Appendix  A.  The  other  simulations  were  run  with  this  program  with  the  unnecessary 
segments  commented  out.  The  program  primarily  consists  of  Input,  Initialization,  Dy¬ 
namic,  State  Estimation,  and  Correction  Segments.  In  the  Input,  the  values  for  the  ve¬ 
hicle's  speed,  the  origin  and  destination  coordinates,  the  predicted  values  for  the  Set  and 
Drift,  and  the  standard  deviations  for  the  various  random  variables  are  entered.  The 
units  used  for  the  distance  and  angle  inputs  are  nautical  miles  and  degrees. 

The  Initialization  segment  the  nautical  miles,  hours,  and  degree  units  arc  conveted 
to  feet,  seconds,  and  radian  units  of  measure.  In  addition,  equidistant  waypoints  and 
course  and  distance  to  destination  are  calcluatcd,  biases  are  added  to  the  predicted  Set 
and  Drift,  and  the  Kalman  Filter  matricies  are  initialized. 

The  Dynamic  segment  calculates  the  actual  x  and  y  coordinates  and  the  current 
heading  angle  which  are  used  as  the  GPS  and  compass  inputs  into  the  filter  by  using  the 
model  of  Figure  4  on  page  8  created  in  Chapter  2. 

The  State  Estimation  updates  i/*.  /i(AV*-i).  and ;  and  then  calls  the  subroutine 
Kalman  [Appendix  B|.  Kalman  performs  the  matrix  operations  and  returns  the  current 
state  estimate,  i**,  the  projected  covariance  of  error,  E^l  k,  and  the  projected  state  esti¬ 
mate,  Finally  the  returned  estimates  of  ux  and  uy  are  used  to  calculate  the  new 


heading  command  in  the  Course  Correction  segment  which,  in  addition,  updates  i*_,  * 
[Eqt.  3.16], 

For  all  the  simulations,  the  vehicle's  ordered  speed  or  speed  through  the  water  is 
maintained  as  a  constant.  This  assumes  that  the  vehicle  has  an  optimum  speed  which 
minimizes  the  amount  of  fuel  consumed  during  a  long  distance  voyage,  which  appears 
to  be  the  trend  in  current  research.  The  program  could  easily  be  modified  for  the  vehicle 
to  maintain  a  constant  over  the  ground  speed.  The  speed  picked  for  all  the  runs  was  7.5 
knots. 

The  predicted  Set  and  Drift  were  held  as  a  constant  for  all  the  runs  at  45  degrees  and 
1.0  knots.  The  awt  and  au)  values  were  those  calculated  in  the  example  on  Page  15  as  .66. 
For  a„  and  <ry,  the  values  used  were  determined  using  Equation  3.18  and  the  DOD  figure 
of  10  meters  for  the  CEP  Thus,  a„  =  ay  =  25.4  feet.  The  value  for  a,3,  chosen  at  random, 
was  1.5  degrees  or  .03  radians.  A  summary  of  the  constants  and  their  values  are  sum¬ 
marized  in  Table  1. 


Table  1.  SUMMARY  OF  VALUES  USED  IN  SIMULATION  RUNS 


CONSTANT 

VALUE  /  UNITS 

T 

600.  Seconds 

Set 

7.5  Knots 

Drift 

45  Degrees 

Orign  Coordinates 

0. 0.0.0  Nautical  Miles 

Destination  Coordinates 

7.0.6.0  Nautical  Miles 

25.4  feet 

25.4  feet 

.03  radians 

CT,v, 

.66  feet  per  second 

a 

IV  V 

.66  feet  per  second 

For  each  simulation  run,  there  are  7  graphs  as  output.  The  first  one  shows  the  ve¬ 
hicle's  overall  track  as  compared  to  the  desired  track  using  the  waypoints  as  a  reference. 
The  remainder  are  plots  of  the  current  state  estimates  shown  against  the  actual  states 
as  a  function  of  time.  The  values  for  the  actual  states  are  graphed  as  solid  lines,  while 
the  estimated  state  graphs  use  a  dashed  line.  For  convenience  to  the  reader,  the  actual 


number  of  figures  are  reduced  by  combining  two  plots  into  each  figure  with  the  excep¬ 
tion  of  the  vehicle's  overall  position  track. 

B.  STATE  ESTIMATION  WITH  NO  BIAS 

In  this  simulation,  the  predicted  Set  and  Drift  are  entered  into  the  vehicle  and  it  is 
instructed  to  maintain  its  course  to  the  destination  point.  There  is  no  added  bias  to  the 
Set  and  Drift  and  the  run  is  performed  for  6000  seconds  or  5  sampling  periods. 

Figure  9  on  page  24  shows  the  vehicle's  actual  track  against  the  intended  track. 
The  waypoints  are  shown  as  a  reference.  As  can  be  seen,  the  vehicle  follows  the  intended 
track  with  no  deviation  which  demonstrates  that  the  program  itself  is  functioning  prop¬ 
erly.  Figure  10  on  page  25  provides  the  estimated  and  actual  position  state  variables 
plotted  both  as  a  function  of  time  and  each  other.  In  Figure  1 1  on  page  26,  the  true 
velocity  state  variables,  .v  andy,  are  again  plotted  as  functions  of  time  and  estimated  vs. 
actual.  Figure  12  on  page  27  plots  the  estimated  and  actual  Set  and  Drift  components. 
ivc  and  uy  in  the  same  manner  as  before.  These  figures  demonstrate  that  the  filter  is 
locking  on  after  only  one  sample  and  maintains  the  lock  throughout  the  run  as  expected. 


o*et 


Figure  10.  Position  Coordinates  With  No  Added  Bias 
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C.  STATE  ESTIMATION  WITH  BIAS  AND  COURSE  CORRECTION 

During  this  simulation,  the  filter's  ability  to  maintain  track  during  a  course  change 
was  determined.  A  bias  of  .2  radians  was  added  to  the  drift  and  .5  knots  was  added  to 
the  Set.  After  the  filter  determined  an  estimate  for  the  Set  and  Drift  components,  the 
simulation  program  added  them  to  a  new  desired  course  to  arrive  at  the  new  heading 
command  and  updated  £*_,■*.  The  new  desired  course  was  calculated  by  comparing  the 
vehicle's  current  position  to  it's  destination  and  subtracting  the  respective  components. 
Aswas  previously  done,  the  length  of  the  run  was  6000  seconds.  The  results  are  dis¬ 
played  in  Figure  13  on  page  29.  Figure  14  on  page  30,  Figure  15  on  page  31.  and 
Figure  16  on  page  32. 

The  figures  demonstrate  the  program  and  filter's  ability  to  correctly  determine  the 
overall  Set  and  Drift  and  give  a  correct  heading  command  to  guide  the  vehicle  to  its 
destination.  As  can  be  seen,  the  filter  successfully  acquires  ux  and  tty  after  only  one 
sample  and  maintains  a  lock  there  after.  This  allows  the  vehicle  to  stay  on  course 
without  having  to  constantly  make  corrections  due  to  bad  estimates  from  the  filter. 
Next,  the  filter's  ability  to  handle  multiple  Set  and  Drift  changes  will  be  tested. 
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Figure  16.  Set  and  Drift  Components  With  Bias  and  Course  Corns.* ion 


D.  STATE  ESTIMATION  WITH  MULTIPLE  SET  AND  DRIFT  CHANGES 

In  this  simulation,  tiie  Set  and  Drift  was  varied  in  a  random  manner  at  the  beginning 
of  every  other  sample.  Values  were  created  using  the  Normal  function  in  DSL  and 
added  to  the  predicted  Set  and  Drift.  The  Normal  function  randomly  generates  num¬ 
bers  that  are  distributed  according  to  a  Gaussian  distribution  function  about  a  mean 
with  a  standard  deviation.  In  this  case  the  mean  was  zero  and  the  standard  deviation 
for  the  Drift  was  7  degrees  or  .12  radians  and  .2  knots  for  the  Set.  The  run  was  for  4200 
seconds  in  order  to  obtain  multiple  variations  at  the  sampling  period  of  600  seconds. 
The  results  were  plotted  and  are  displayed  in  the  same  manner  as  the  previous  two  sim¬ 
ulations. 

In  Figure  17  on  page  34.  the  vehicle's  actual  track  is  off  the  intended  track  by  as 
much  as  500  feet  which  could  be  critical  in  any  actual  mission  performed  by  a  vehicle 
equipped  with  this  system  as  its  only  navigation  source.  The  rest  of  the  graphs  as  shown 
in  Figure  18  on  page  35.  Figure  19  on  page  36.  and  Figure  20  on  page  37  show  the  fil¬ 
ter's  ability  to  track  any  deviations  in  the  Set  and  Drift  experienced  by  the  vehicle  with 
only  one  sample  after  the  change. 
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Lire  18.  Position  Coordinates  With  Multiple  Set  and  Drift  Changes 
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Figure  19.  Components  With  Multiple  Set  and  Drift  Changes 


Figure  20.  Set  and  Drift  Components  With  Multiple  Set  and  Drift  Changes 


E.  STATE  ESTIMATION  WITH  MEASUREMENT  NOISE 

In  this  simulation  noise  was  added  to  the  measurements  of.v,  y,  and  i f/  in  order  to 
determine  how  the  Kalman  Filter  would  respond.  This  noise  was  added  using  the 
Normal  function  described  in  the  previous  section.  A  different  function  was  generated 
for  each  measurement  with  unique  seed  numbers,  in  order  to  preserve  their  independ¬ 
ence,  and  zero  mean  about  the  actual  value.  The  resulting  graphs  are  displayed  in 
Figure  21  on  page  40,  Figure  22  on  page  41,  Figure  23  on  page  42,  and  Figure  24  on 
page  43.  The  run  time  was  4200  seconds  and  as  in  the  previous  simulation  the  Set  and 
Drift  were  changed  every  other  sample. 

The  graphs  show  the  same  results  which  were  achieved  in  the  third  simulation  with 
some  small  differences.  The  vehicle's  intended  and  actual  tracks  are  much  closer  than 
before  which  is  due  to  a  lesser  amount  of  deviation  in  the  Set  and  Drift  variables.  A 
close  look  at  the  plots  of  the  state  variables  show  some  variation  between  their  estimated 
and  actual  values  which  was  not  present  before.  These  are  the  errors  produced  by  the 
measurement  noise.  In  order  to  see  the  actual  differences,  three  tables  were  generated 
which  show  the  actual  and  estimated  values  against  time.  The  position  coordinates.  X 
and  Y,  are  shown  in  Table  2  on  page  39,  .randy;  are  in  Table  3  on  page  39.  and 
ux  and  uy  are  in  Table  4  on  page  39. 

From  the  tables,  the  errors  range  from  3  feet  up  to  21  feet  for  x  andy,  are  less  than 
.02  feet  per  second  for  .r  andy,  and  vary’  up  to  .IS  feet  per  second  for  ux  and  uy.  Note 
also  that  during  the  periods  when  there  is  no  change  in  the  Set  and  Drift,  the  estimates 
show  a  significant  improvement.  Errors  such  as  these  may  be  tolerable  depending  on  the 
vehicle's  assigned  mission.  The  best  way  to  minimize  the  error  would  be  to  take  multiple 
GPS  readings  in  order  to  obtain  a  better  fix.  But  this  would  increase  the  amount  of  time 
that  the  vehicle  would  spend  on  the  surface. 
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Table  2.  DIFFERENCES  IN  POSITION  COORDINATES 


TIME 


TIME 


ACTUAL 


5092.4 


10056. 


14SS0. 


19803. 


24S69. 


29S22. 


S. 


ESTIMATED 

ACTUAL 

ESTIMATED 

5098.4 

4103.5 

4131.1 

10060. 

8351.7 

8368.0 

14S90. 

12712. 

1 2708. 

19801. 

16962. 

16971. 

24863. 

21097. 

21103. 

29831. 

25359. 

25367. 

34525. 

29864. 

29S70. 

600. 


1200. 


1S00. 


400. 


3000. 


600. 


4200. 


Table  3.  DIFFERENCES  IN  VELOCITY  COMPONENTS 


ACTUAL 


8.4S62 


8.0398 


8.0398 


8.4441 


3000. 

8.4441 

3600. 

7.8266 

4200. 

7.8266 

ESTIMATED 


8.5048 


8.0520 


8.0520 


S.IS57 
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8.2801 


7.8265 


ACTUAL 


6.8423 


7.2664 


7.2666 


6.8931 


6.S931 


7.5081 


7.508 1 


ESTIMATED 


6.S961 


7.2318 


7.23  IS 


7.1053 


6.SS7S 


7.1072 


7.5015 


Table  4.  DIFFERENCES  IN  SET  AND  DRIFT  COMPONENTS 


TIME 

ux 

UY 

ACTUAL 

ESTIMATED 

ACTUAL 

ESTIMATED 

600. 

-.92089 

-.90220 

-1.3892 

-1.3354 

1200. 

-1.1535 

-1.1414 

-1.2030 

-1.4073 

1800. 

-1.1535 

-1.1414 

-1.2o30 

-1.2378 

2400. 

-.91472 

-1.1730 

-1.3932 

-1.1811 

3000. 

-.91472 

-.92496 

-1.3932 

-1.39S5 

3600. 

-1.3426 

-.88910 

-.98751 

-1.3884 

4200. 

-1.3426 

-1.3428 

-.98751 

-.99409 

xm 
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Figure  24.  Set  and  Drift  Components  With  Measurement  Noise 
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V.  CONCLUSION 


A.  ANALYSIS 

The  simulation  results  from  Chapter  4  demonstrated  the  Extended  Kalman  Filter's 
ability  to  successfully  identify  the  total  Set  and  Drift  that  an  underwater  vehicle  could 
experience  on  its  journey.  However,  there  are  some  limitations  which  have  appeared. 
First,  if  the  magnitude  of  the  Set  and  Drift  vary  extensively  between  samples,  then  the 
vehicle's  ability  to  navigate  precise  tracks  is  severely  limited.  There  are  three  major 
factors  which  affect  how  far  the  vehicle  will  be  driven  oil  its  intended  course;  the  mag¬ 
nitude  of  the  Set  and  Drift,  the  vehicle's  ordered  speed,  and  the  time  between  samples. 
There  is  no  control  over  the  first  factor  ’  ut  there  is  some  latitude  over  which  the  oper¬ 
ator  can  control.  Increasing  the  speed  and  decreasing  the  sampling  interval  will  allow 
less  deviation  from  an  intended  track.  However,  both  the  resulting  increase  in  power 
consumption  and  the  increase  in  the  probability  of  detection  will  have  to  be  taken  into 
account. 

Another  major  limitation  with  using  this  system  as  a  vehicle's  sole  navigation  source 
is  that  the  covert  aspect  of  an  underwater  vehicle  becomes  lost  awhen  the  GPS  fixes  are 
taken.  Because  of  the  frequencies  at  which  GPS  operates  have  very  little  ability  to  travel 
through  water,  the  vehicle's  antenna  must  break  through  the  water's  surface.  To  mini¬ 
mize  the  risk  of  detection,  a  system  would  have  to  be  designed  which  would  allow  the 
antenna  to  be  raised  and  lowered  from  a  safe  depth.  An  alternative  would  be  to  use  the 
Omega  Navigation  System,  whose  frequencies  can  be  reach  down  to  about  40  feet,  as  a 
prime  source  and  use  GPS  as  an  update  at  regular  intervals  or  whenever  the  Omega  data 
is  suspect.  This  technique  could  allow  the  vehicle  to  refrain  from  surfacing  as  much  and 
at  a  rate  comparable  to  that  required  in  updating  an  IMU  system. 

B.  APPLICATIONS 

1.  Back  up  to  an  IMU 

The  most  obvious  place  for  this  design  is  as  a  backup  to  an  IMU  system 
onboard  an  underwater  vehicle  which  would  take  over  in  event  of  a  failure.  Current 
design  philosophy  seems  to  be  centered  on  using  conventional  dead  reckoning  systems 
in  which  the  angles  and  components  are  calculated  using  trigonometric  functions.  These 
type  of  complex  calculations  are  difficult  for  a  microprocessor  to  accomplish  and  ma¬ 
chine  roundolF  errors  can  occur  if  the  ansiles  are  close  toeether.  This  Extended  Kalman 


Filter  design  requires  only  one  trigonometric  function  [Eqt.  3.15]  and  the  matrix 
calculations  are  ideally  suited  for  a  microprocessor. 

2.  Mine  Location 

Because  this  system  is  lightweight,  inexpensive,  and  has  very  little  material 
which  will  disturb  a  magnetic  field,  a  large  fleet  of  plastic  mine  locating  escorts  could 
be  developed.  These  escorts  could  lead  ships  through  a  mined  area  by  locating  suspected 
mine  positions.  As  such,  the  vehicle  would  be  composed  of  a  body,  a  propulsion  unit, 
a  simple  attitude  control  section,  and  a  detector.  This  detector  would  be  the  most  ex¬ 
pensive  component  and  it  could  take  the  form  of  a  high  resolution  sonar  or  a  magnetic 
anomaly  detector.  The  shape  of  the  vehicle  could  be  designed  that  would  reduce  any 
pressure  waves  which  would  set  off  any  pressure  sensitive  mines.  The  antenna  system 
would  not  have  to  be  as  complicated  as  mentioned  previously  because  the  need  to  op¬ 
erate  covertly  would  be  erased.  The  communication  link  could  be  fiber  optic  and  go  to 
a  mother  ship  or  a  group  of  mother  ships  working  together. 

3.  Long  Range  Reconnaisance 

A  variant  of  the  mine  locating  vehicle  discussed  above,  would  be  as  a  long  range 
reconnaisance  vehicle  with  the  mine  detector  replaced  with  some  other  sensor  such  as  a 
communications  receiver  or  an  infrared  detector.  The  missions  would  be  limited  to  those 
which  do  not  require  precise  navigation  such  as  coast  or  ship  surveillance.  The  advan¬ 
tages  to  using  this  system  is  that  the  power  needed  to  run  an  I  MU  would  be  eliminated 
and  and  the  costs  significantly  reduced.  Communication  could  be  in  the  form  of  fiber 
optics  for  realtime  information.  Radio  or  data  storage  systems  could  be  used  for  non¬ 
realtime.  Delivery  and  retrieval  could  be  accomplished  through  the  use  of  a  submarine, 
a  ship,  or  small  boats. 

C.  RECOMMENDATIONS  FOR  FUTURE  RESEARCH 

The  following  areas  present  valid  opportunities  for  useful  expansion  of  this  work: 

•  Develop  an  antenna  and  support  system,  in  order  to  receive  the  GPS  signals  in  the 
presence  of  low  to  medium  sea  state  and  allow  the  vehicle  to  remain  at  depth. 

•  Build  a  prototype  Extended  Kalman  Filter  and  test  it  as  a  back  up  on  an  existing 
autonomous  underwater  vehicle. 


APPENDIX  A.  SIMULATION  PROGRAM 


This  program  simulates  an  underwater  vehicle  traveling  through  the  water  and  is 
written  in  DSL.  The  Dynamic  section  mimics  the  vehicle's  true  dynamics,  which  were 
modeled  in  Chapter  2  as  a  second  order  system,  and  thereby  calculates  the  vehicles  true 
position  and  heading  angle.  The  Sample  section  samples  the  system's  position  and 
heading  angle  every  T  seconds  and  these  measurements  are  sent  to  the  subroutine 
Kalman  which  estimates  the  states.  After  the  estimates  are  returned,  the  program  cal¬ 
culates  a  new  heading  command  and  updates  the  predicted  state  vector  XP. 


TITLE  VEHICLE  SIM  WITH  KALMAN  CORRECTION  TO  DEST  /  VAR  F  AND  BIAS 
PARAM  VEL=7.  5  ,FVEL=1.  ,FANG=45.  ,  N=I0,N1=7  ,N2=13  ,N3=3 
PARAM  AX=0.  , AY=0.  ,  BX=14.  0  ,  BY=12.  0 

PARAM  SIGCA=0. 3iSIGCV=0. 3  ,SIGX2=100. ,SIGY2=100.  ,SGYAW2=. 1  .... 

SIGCY=. 1  ,SIGCX=. 1 , SIGX=10. ,SIGY=10. ,SIGYAW=. 01 
D  DIMENSION  WAPNTXC 0:  10) ,WAPNTY( 0:  10) 

D  DIMENSION  F(6,6) ,XHAT(6) ,XP(6) ,Y( 3) ,Q(2,2) ,R(3 ,3) ,P(6,6) ,H( 3) , 

D  1  HP( 3,6) ,G( 6 , 2) ,XM( 6) 

EXCLUDE  IER , F , XHAT , XP , Y , Q , R , P , H , HP , G , XM 
D  DATA  F , XHAT , HP , Y , Q , R , P , H , XP , G  /  133  *0.  / 

FIXED  I , J, N ,N1 ,N2 ,N3 
INITIAL 

Al=2. 

Tl=5. 

A=0. 

TIMEW=0. 

T=DELS 

* 

*  ADDITION  OF  ERROR  TO  PREDICTED  CURRENT  VELOCITY  AND  ANGLE 
FANGS=( F ANG+ 180.  )*DEGRA  +  .2 
FVELS=FVEL 

FVELS=(FVELS*6000.  )/3600. 

F  VE  L=  ( F  VE  L*  6  0  0  0 .  )/3600. 

VEL=( VEL‘V6000.  )/3600. 

BX=BX*6000. 

BY=BY*6000 

UXD0TS=FVELS*C0S( FANGS) 

UYD0TS=FVELS*SIN( FANGS ) 

DC=ATAN2( BY-AY , BX-AX) 

DIST=SQRT( ( BX-AX)**2+( BY-AY)**2) 

INT=DIST/10. 

DO  10  1=0,10 

WAPNTXC I)=I  *  INT*COS(DC) 

WAPNTY ( I ) = I  *  INT*SIN(DC) 

WX=WAPNTX( I ) 

WY=WAPNTY( I ) 
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CALL  SAVE( S3) 

10  CONTINUE 

INITIALIZE  KALMAN  MATRICIES 

INITIALIZE  PHI 

DO  11  1=1,6 
F(  I ,  I  )=1. 

11  CONTINUE 
F( 1 , 2)=T 
F  ( 3 , 4 ) =T 

INITIALIZE  Q 

Q( 1 , 1)=SIGCX 
Q( 2 , 2)=SIGCY 

INITIALIZE  GAMMA 1 

G( 1 , 1)=T 
G(  2 , 1)=1. 

G( 3 , 2)=T 
G(  4 , 2)  =  1. 

G(  5 , 1)=1. 

G(  6 , 2)  =  1. 

INITIALIZE  R 

R( 1 , 1)=SIGX2 
R( 2 , 2)=SIGY2 
R(3,3)=SGYAW2 

INITIALIZE  XHAT  WITH  PREDICTED  VALUESOF  CURRENTCOMPONENTS 

XHAT( 5 )=0. 

XHAT( 6)=0. 

XHAT(5)=FVEL  *  COS( (FANG+180.  )*DEGRA) 

XHAT( 6)=FVEL  *  SIN( (FANG+180.  )*DEGRA) 


INITIALIZE  HP 


HP(  1 , 1)  =  1. 
HP(  2 , 3)  =  1. 


*  DETERMINATION  OF  ACTUAL  X  AND  Y  COORDINATES( SUNS  INPUT) 

DERIVATIVE 

HDGERR=YAW  -  HDG 
K=-4.  4 1*HDGERR 
YAWDOT=REALPL( 0.  ,.25,K) 

YAW= I NTGRLf 0 .  , YAWDOT ) 

XDOT=VEL*COS(YAW) 

XDOTS=XDOT+UXDOTS 
YDOT=S I N ( YAW ) * VE  L 
YDOTS=UYDOTS+YDOT 


Y1=INTGRL(0.  , YDOTS) 

X=INTGRL(0.  , XDOTS ) 

SAMPLE 

IFCTIME. EQ. 0. )THEN 
FANG=FANG  *  DEGRA 
IF(SIN(FANG)*SIN( DC).  GT.  0)THEN 
ALPHA=P I - AB S ( F ANG ) +DC 
BETA=AS IN( FVEL*S IN( ALPHA) / VEL) 

HDG=DC  +  BETA 
ELSE 

ALPHA=PI-ABS(FANG) -DC 
BETA=AS INC FVEL*S INC ALPHA) /VEL) 

HDG=DC  -  BETA 
ENDIF 

*  HDG=DC 

XHAT( 2 ) =VEL*COS ( HDG ) 

XHAT( 4 )=VEL*S IN( HDG ) 

XP(2)=VEL*C0S(HDG) 

XP(4)=VEL*SIN(HDG) 

Y( 3)=HDG 
DO  25  1=1,6 
XM(I)=XPCI) 

25  CONTINUE 

CALL  KALMANC A , XHAT ,F,XP,Y,Q,R,P,H,HP,G, IER) 
ELSE 

A=A  +  1. 

*  DETERMINATION  OF  REQUIRED  COURSE  TO  DESTINATION 

30  DWY=WAPNTY( 10 ) -  Y1 

DWX=WAPNTXC 10) -  X 
DC=ATAN  2  C  DWY , DWX ) 

*  CALCULATE  FORCE  COMPONENTS , UANG  AND  UVEL 

*  UPDATE  H,  Z,  AND  HP  MATRICIES 

HC1)=XPC1) 

H( 2)=XP( 3) 

HC3)=ATAN2CXP(4)-XP(6) ,XP(2)-XPC5)) 

DEN=C  XP( 2) -XP( 5) )**2  +  CXP(4)-XP(6) )**2 
HPC  3,2 )=C  XP( 6 ) -XP(4) )/DEN 
HP( 3 , 4)=( XP( 2) -XP( 5 ) ) /DEN 
HP( 3 , 5 )=-HP( 3,2) 

HPC  3 , 6)  =  -HPC  3 ,4) 

YC1)=X  +  N0RMALCN1 , 0 , SIGX) 

Y( 2)=Y1  +  NORMALCN2,0,SIGY) 

Y( 3) =YAW+  NORMALC  N3 , 0 , SIGYAW) 

DO  35  1=1,6 
XMf I)=XP(I) 

35  CONTINUE 


KALMAN  FILTER  UPDATE  OF  XHAT 

CALL  KALMANC A, XHAT, F,XP,Y,Q,R,P,H, HP, G, IER) 

CALCULATION  OF  ESTIMATED  CURRENT  VELOCITY  AND  ANGLE  FROM  UPDATED  XHAT 


UVEL=SQRT( XHAT( 5 )**2+XHAT( 6 )**2) 


'THyvrmvvv,jy\.  'nrwcr^.jrvrk.*  tfsfst  *'  v  ^  v  v  v  w  «ct  v.  «■,  r*  ct  nr  v  ■.■  *.-  *l-  v  *.-  v  ■«.-  v  ^  v  v-  v ».-  v.uva-'.m  %w,v  vyi 


* 

Vc 


UANG=ATAN( XHAT( 6 )/XHAT( 5 ) ) 

CALCULATION  OF  STEERED  COURSES  TO  WAYPOINT  AND  DESTINATION  * 

IF(  A.  EQ.  Al-1.  )THEN 
IF(SIN(UANG)*SIN(HDG).  GT.  0)THEN 
ALPHA=P I - AB  S ( UANG ) +DC 
BETA=AS  IN(  UVEL‘VS  IN(  ALPHA )  /  VEL) 

HDGD=DC  +  BETA 
ELSE 

ALPHA=PI -ABS( UANG) -DC 
BETA=ASIN(UVEL*SIN( ALPHA) /VEL) 

HDGD=DC  -  BETA 
END  IF 

UPDATE  X(K+1/K)  FOR  CRSE  CHANGE 
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XP( 2)=  VEL  *  COS(HDGD)  +  XHAT(5) 
XP(1)=  T  *  XP( 2)  +  XHAT(l) 

XP( 4)=  VEL  *  SIN(HDGD)+  XHAT(6) 
XP( 3)=  T  *  XP( 4)  +  XHAT( 3) 
HDG=HDGD 
ELSE 
HDG=HDG 
END  IF 
ENDIF 

DYNAMIC 

IF( A.  EQ.  A1)THEN 

B IAS=NORMAL( N ,  0.  ,.3) 
UXDOTS=FVELS*COS(  FANGS+BIAS) 
UYDOTS=FVELS*S IN( FANGS+B IAS ) 
Al=Al+2. 

T1=TIME  +  2. 

ELSE 

UXD0TS=UXD0TS 

UYD0TS=UYD0TS 

ENDIF 

IF(TIME.  LT.  T1)CALL  SAVE(S2) 
X1KK=XHAT( 1) 

X2KK=XHAT( 2) 

X3KK=XHAT( 3) 

X4KK=XHAT( 4) 

X5KK=XHAT( 5 ) 

X6KK=XHAT( 6) 

X1KKM1=XM( 1) 

X2KKM1=XM( 2) 
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X3KKM1=XM( 3 ) 

X4KKM1=XM( 4) 

X5KKM1=XM(5) 

X6KKM1=XM( 6) 

40  IF(X. GE.BX)  CALL  ENDRUN 

CONTROL  FT.NTIM=9000.  ,DELS=600. 

METHOD  ADAMS 

SAVE  (SI)  600.  ,X1KK,X2KK,X3KK,X4KK,X5KK,X6KK,.  . 

X1KKM1 ,X2KKM1 ,X3KKM1 .X4KKM1 ,X5KKM1 ,X6KKM1 
SAVE  ( S 2 )  600.  ,X, Y1 , UXDOTS ,UYD0TS ,XDOTS , YDOTS 
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SAVE  ( S3)4000. ,WX,WY 

PRINT  600 ,HDG,X, Y1 , UXDOTS , UYDOTS 

GRAPH  (G1/S2 ,DE=TEK618)X( SC=8.  4E3) ,Y1(SC=10.  5E3,LO=0.  ) 

GRAPH  (G2/S3 ,DE=TEK618 ,OV)WX( SC=8.  4E3 ,AX=OMIT) , .  .  . 

VY(MA=4,SC=10.  5E3 , AX=OMIT) 

GRAPH  ( G3/S2 , DE=TEK6 18 ) TIME , X( LI=1 , AX=OMIT , L0=0 ) 

GRAPH  ( G4/S 1 , DE=TEK6 18 , OV ) TIME , X1KK(  LI=2 ,  LOO ) , X1KKM1  ( LI=3 ,  L0=0 ) 

GRAPH  ( G5 / S  2 , DE=TEK6 1 8 ) TIME , Y 1 ( LI= 1 , AX=OMIT , L0=0 ) 

GRAPH  ( G6/S1 ,DE=TEK618 ,0V)TIME ,X3KK( LI=2 ,LO=0) ,X3KKM1( LI=3 , L0=0) 
GRAPH  ( G7/S2 ,DE=TEK618)TIME ,XD0TS( AX=0MIT, LI=1 ,LO=0. ,SC=1.8) 

GRAPH  ( G8/S1 ,DE=T£K618 ,OV)TIME ,X2KK( LI=2 , LG=0 , SC=1.  8),... 

X2KKM1( LI=3 , L0=0. ,SC=1. 8) 

GRAPH  ( G9/S2 , DE=TEK6 18 )TIME , YD0TS( AX=OMIT , LI=1 , L0=- 1.  5 , SC=1. 5) 

GRAPH  ( G10/S1 ,DE=TEK618 ,0V)TIME ,X4KK( LI=2 ,LO=- 1.5, SC=1.  5) . 

X4KKM1( LI=3 , L0=- 1. 5 , SC=1.  5) 

GRAPH  ( G1 1/S2 , DE=TEK6 18 )TIME , UXDOTS( AX=OMIT , LI=1 , L0=-2.  0,SC=. 5) 

GRAPH  (G12/S1 ,DE=TEK618 ,0V)TIME,X5KK( LI=2 ,L0=-2.  0 ,SC=.  5) .  . 

X5KKM 1 ( LI=3 , LO=- 2 .  0,SC=.  5) 

GRAPH  ( G13/S2 , DE=TEK6 18 )TIME , UYD0TS( AX=OMIT , LI=1 , L0=-2. 0 , SC*. 5 ) 

GRAPH  ( G14/S1 ,DE=TEK618 ,OV)TIME ,X6KK( LI=2 ,L0=-2.  0 ,Sl=. _ ) , . . . 

X6KKMl(LI=3,LO=-2.  0,SC=. 5) 

LABEL  (ALL)  KALMAN  CORRECTION  TO  DEST  /BIAS*  VAR/CUR  CHG=2/MSMT  ERROR/ 

END 

STOP 


APPENDIX  B.  KALMAN  FILTER  PROGRAM 


This  program  is  written  in  Fortran  77  and  performs  the  necessary  matrix  calcu¬ 
lations  for  the  extended  Kalman  filter  developed  in  Chapter  3.  The  routine  calls  three 
IMSLDP  subroutines,  LEQT1F,  VMULFF,  and  VMULFP  which  accomplish  the  ma¬ 
trix  multiplication  operations.  In  order  for  Kalman  to  be  compatible  with  DSL,  it  was 
written  in  double  precision.  After  performing  the  calculations,  this  subroutine  returns 
the  predicted  covariance  matrix  (P),  the  updated  state  estimate  vector  (XI  IAT),  and  the 

predicted  state  vector  (XP)  to  the  callina  program. 

C 

SUBROUTINE  KALMAN  ( A ,X, PHI ,XKKM1 , Z ,Q,R , P ,HK,HKP .GAMMA, IER) 

C  SPECIFICATIONS  FOR  ARGUMENTS 

IMPLICIT  REAL*8(A-H,0-Z) 

INTEGER  IER 

DIMENSION  X( 6) ,XKKM1( 6) ,PHI(6,6) ,Z(3) ,Q( 2 , 2) ,R( 3 , 3) , 

1  HK(3),HKP(3,6),T1(6,6),T2(6,6),T3(6),P(6,6), 

2  GAMMA(6,2) ,G(6,3) 

INTEGER  I.J.N.NX.NY.L 

IER  =  0 

N=6 

NY=3 

NX=1 

L=2 

K=A 

WRITE( 6 , 2) ( (HKP( I , J) , J=1 , 6) , 1=1 ,3) 

WRITE( 6 ,5 ) (XKKM1( J) ,  J=l,6) 

WRITE(6,6)(HK( J) ,  J=l,3) 

WRITE( 6 , 12)( Z( J) ,  J=1 ,3) 

C  CALCULATE  P  IF  K  =  ZERO 

IF  (A  .EQ.  0)  THEN 
WRITE ( 6 , 3) ( ( PHI( I , J) , J=1 , 6) , 1=1,6) 

WRITE ( 6,7)((Q(I,J),J=I,2),I=1,2) 

WRITE( 6 , 8) ( (GAMMA( I , J) ,  J=l,2),  1=1,6) 

CALL  VMULFF  (  GAMMA,  Q,  N,  L,  L,N,L,T2 ,N, IER) 

CALL  VMULFP  (T2,  GAMMA,  N,  L,  N,N,N,  P,N,IER) 

C  DO  15  1=1,6 
C  P( I , I )=4000. 

C  15  CONTINUE 

WRITE( 6 , 13) ( ( P( I , J) , J=1 ,6) , 1=1 ,6) 

ELSE 

CONTINUE 
END  IF 

C  CALCULATE  GAIN  MATRIX  G(K) 

CALL  VMULFP  (HKP ,  P,NY,  N,  N,NY,N,T2,N, IER) 

CALL  VMULFP  ( T2 , HKP , NY ,  N,NY,N,NY,T1 ,N, IER) 

DO  35  I  =  1 ,NY 
DO  30  J  =  1 ,NY 

Tl( I , J)  =  T 1 ( I , J)  +  R( J, I) 

30  CONTINUE 


35  CONTINUE 

CALL  LEQT1F(T1 ,N,NY,N,T2 ,0 ,T3 , IER) 

IF  (IER  .EQ.  0)  GO  TO  40 
IER  =  130 
GO  TO  9000 
40  DO  50  I  =  1  ,NY 

DO  45  J  =  1,N 

G( J, I)  =  T2( I , J) 

45  CONTINUE 

50  CONTINUE 

WRITE( 6 ,9)( (G( I , J) ,  J=1 , 3) , 1=1 ,6) 

C  CALCULATE  P(K/K) 

CALL  VMULFF  ( G , HKP ,  N,NY,  N,N,NY,T2 ,N, IER) 
CALL  VMULFF  (T2,  P,  N,  N,  N ,N ,N,T1 ,N, IER) 

DO  75  I  =  1,N 
DO  70  J  =  1  ,N 

P(I,J)  =  P(I,J)  -  T1(I,J) 

70  CONTINUE 

75  CONTINUE 

WRITE ( 6 , 11) ( ( P( I , J) ,  J=1 , 6) ,  1=1,6) 

C  CALCULATE  X(K/K) 

DO  60  I  =  1 ,NY 

T3(  I )  =  2(1)  -  HK( I) 

60  CONTINUE 

CALL  VMULFF  (G,T3,  N,NY,NX,N,NY,T2,N,IER) 

DO  65  I  =  1,N 

X(I)  =  XKKMl(I)  +  T2( 1,1) 

65  CONTINUE 

WRITE(6,4)(X( J) ,  J=1 ,6) 

C  CALCULATE  P(K+1/K) 

10  CALL  VMULFF  (  PHI,  P,  N,  N,  N,N,N,T1 ,N, IER) 
CALL  VMULFP  (Tl,  PHI,  N,  N,  N,N,N,  P,N,IER) 
CALL  VMULFF  (  GAMMA,  Q,  N,  L,  L,N,L,T2,N, IER) 
CALL  VMULFP  (T2,  GAMMA,  N,  L,  N,N,N,T1 ,N, IER) 
DO  25  I  =  1,N 
DO  20  J  *  1,N 

P(I,J)  =  P(I,J)  +  Tl( I , J) 

C  P(I,J)  =  P(I,J) 

20  CONTINUE 
25  CONTINUE 

WRITE ( 6,1)((P(I,J),  J=1 ,6) ,  1=1,6) 

C  CALCULATE  X(K+1/K)=XKKM1 

CALL  VMULFF  (  PHI,  X,  N,  N,NX ,N,N,T1 ,N, IER) 

DO  80  I  =  1 ,N 

XKKMl(I)  =  Tl( 1,1) 

80  CONTINUE 
GO  TO  9005 
9000  CONTINUE 

CALL  UERTST  ( IER, 6HFTKALM) 

9005  RETURN 

1  FORMAT( / , 1  PKP1K=' ,/,6(3X,F12.4,3X)) 

2  F0RMAT( / , '  ZP=' ,/,6(3X,F12.  4,3X)) 

3  FORMAT( / , '  PHI=' , / ,6( 3X,F12.  4,3X) ) 

4  FORMAT ( / , 1  XKK='  ,/ ,6(3X,F12. 4, 3X) ) 

5  FORMAT( / , 1  XKKM1=' , / ,6( 3X,F12.  4 , 3X) ) 

6  FORMAT( / , '  HK=' ,/,3(3X,F12.  4,3X)) 
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FORMAT ( / , 
FORM AT ( / , 
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FORM AT ( / , 
FORMAT( / , 
FORMAT ( / , 
END 


Q=  , / , 2( 3X,F12.  4 , 3X) ) 

GAMMA= ' , / , 2( 3X.F12.  4,3X)) 
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